#include "Spiral.h"

namespace poly
{
	Spiral::Spiral() :
		Joint(), torqueDensity( 0 ), friction( 0 ), deviation( 0 ), axis( 0, 0, 1 )
	{
	}

	void Spiral::apply( float dt )
	{
		Joint::apply(dt);
		float3 worldAxis = axis * rigidBody1->orientation;
		float w1 = dot(rigidBody1->rotVel, worldAxis);
		float w2 = dot(rigidBody2->rotVel, worldAxis);

		float3 torque = (deviation * torqueDensity - (w2-w1) * friction) * worldAxis;
		deviation +=  (w2-w1)*dt;

		rigidBody1->addTorque( +torque );
		rigidBody2->addTorque( -torque );
	}
}
